import rospy
from geometry_msgs.msg import PoseStamped, TwistStamped


setpoint_msg = PoseStamped()
# setvelocity_msg = TwistStamped()
position = [PoseStamped() for i in range(3)]
position_pub = [None for i in range(3)]
# velocity = [TwistStamped() for i in range(3)]
# velocity_pub = [None for i in range(3)]
z_bias = [0.5,0.38,0.38]
def callback1(data,i):
    #rospy.loginfo(str(data.pose.position.x)+','+str(data.pose.position.y)+','+str(data.pose.position.z))
    position[i] = data

# def callback2(data,i):
#     #rospy.loginfo(str(data.pose.position.x)+','+str(data.pose.position.y)+','+str(data.pose.position.z))
#     velocity[i] = data

rospy.init_node('transfer_f450')
for i in range(3):
    rospy.Subscriber("/vrpn_client_node/parallel_f450_"+str(i+1)+"/pose", PoseStamped, callback1,i)
    # rospy.Subscriber("/vrpn_client_node/parallel_f450_"+str(i+1)+"/twist", TwistStamped, callback2,i)
 # spin() simply keeps python from exiting until this node is stopped
for i in range(3):
    position_pub[i] = rospy.Publisher("/UAV"+str(i)+"/mavros/vision_pose/pose", PoseStamped, queue_size=2) #creates a publisher object
    # velocity_pub[i] = rospy.Publisher("/UAV" + str(i) + "/mavros/local_position/velocity_local", TwistStamped,
    #                                   queue_size=2)  # creates a publisher object
rate = rospy.Rate(15) #the rate will take care of updating the publisher at a specified speed - 50hz in this case
#loop_count = 0

while True:
    for i in range (3):
        setpoint_msg = position[i]
        setpoint_msg.pose.position.z -= z_bias[i]
        setpoint_msg.header.stamp = rospy.Time.now()
        # setvelocity_msg = velocity[i]
        # setvelocity_msg.header.stamp = rospy.Time.now()
        #setpoint_msg.header.seq = loop_count
        #setpoint_msg.header.frame_id = '/world'
        #rospy.loginfo(str(posx)+','+str(posy)+','+str(posz))
        position_pub[i].publish(setpoint_msg)
        # velocity_pub[i].publish(setvelocity_msg)
        #loop_count = loop_count + 1
    rate.sleep()
#creates a message object of type pose stamped #http://docs.ros.org/api/geometry_msgs/html/msg/PoseStamped.html

